#include "Equalizer.h"
#include <iostream>
#include <math.h>

#define M_PI 3.14159265358979323846

static inline double value2db(double value) { return value > 0 ? log10(value) * 20.0 : 0.0; }

static inline double db2value(double db) { return pow(10.0, db / 20.0); }

static inline double db2value(double db, int mul) { return pow(10.0, db / (mul * 20.0)); }

static inline float value2db(float value) { return value > 0 ? log10f(value) * 20.0f : 0.0f; }

static inline float db2value(float db) { return powf(10.0f, db / 20.0f); }

static inline float db2value(float db, int mul) { return powf(10.0f, db / (mul * 20.0f)); }

void CEqualizer::bp2(float *a, float *b, double fc, double q) {
    double th = 2 * M_PI * fc;
    double C = (1 - tan(th * q / 2)) / (1 + tan(th * q / 2));

    static const double bp2mult = 1;
    a[0] = float(bp2mult * (1 + C) * cos(th));
    a[1] = float(bp2mult * -1 * C);

    b[0] = float(bp2mult * (1.0 - C) / 2.0);
    b[1] = float(bp2mult * -1.0050);
}

void CEqualizer::init(const EQSettings *cfg, double freq) {
    reset();
    memcpy(&m_settings, cfg, sizeof(m_settings));

    static const double qmult = 1;
    for (int i = 0; i < MAX_CHL_NUMS; i++) {
        for (int j = 0; j < BAND_NUMS; j++) {
            float db = (cfg->highdb - cfg->lowdb) * (&cfg->eqvl[0])[j] / 200.0f + cfg->lowdb;
            g[i][j] = (float) (qmult * (db2value(db, 100) - 1.0));
        }
    }

    K = BAND_NUMS;
    while (((&cfg->freq[0])[K - 1] / 100.0) > freq / 2.3)
        K--;

    const double Q = 1.2247449;
    for (int k = 0; k < K; k++)
        bp2(a[k], b[k], ((&cfg->freq[0])[k] / 100.0) / freq, Q);

}

short CEqualizer::equalize(const float *g, short in, unsigned int ci) {
    float yt = (float) in;
    for (int k = 0; k < K; k++) {
        float *wq = this->wq[ci][k];
        float w = yt * b[k][0] + wq[0] * a[k][0] + wq[1] * a[k][1];
        yt += (w + wq[1] * b[k][1]) * g[k];
        wq[1] = wq[0];
        wq[0] = w;
    }

    if (yt > 32767)
        return 32767;
    else if (yt < -32767)
        return -32767;

    return (short) yt;
}

void CEqualizer::process(short *p, int nums, int chls) {
    unsigned int ci = chls;
    while (ci--) {
        const float *g = this->g[ci];

        short *inout = p + ci;
        short *end = inout + chls * nums;
        while (inout < end) {
            *inout = equalize(g, *inout, ci);
            inout += chls;
        }
    }
}

void CEqualizer::reset(void) { memset(wq, 0, sizeof(wq)); }

void CEqualizer::upate(int band, int value) {
    m_settings.eqvl[band] = value;
    static const double qmult = 1;
    float db =
            (m_settings.highdb - m_settings.lowdb) * m_settings.eqvl[band] / 200.0f +
            m_settings.lowdb;
    g[0][band] = (float) (qmult * (db2value(db, 100) - 1.0));

}
